import json
import cv2
import os
import numpy as np
from tqdm import tqdm
from collections import deque
import matplotlib.pyplot as plt


def read_json(path):
    dict = json.load(open(path))
    return dict

def np_move_avg(a, n, mode = "same"):
    return (np.convolve(a, np.ones((n,)) / n, mode = mode))

def calculate_dis(center1,center2):
    center1 = np.array(center1)
    center2 = np.array(center2)
    return np.sqrt(np.sum(np.square(center1 - center2)))


# ball json -> dict
ballDict = read_json("./json/ball_personid.json")
personDict = read_json("./json/poseAdd2Id.json")

# color for different person
# COLORS = [[0, 255, 0], [0, 0, 255], [255, 0, 255], [255, 255, 0], [0, 255, 255]]
COLORS = [[120, 220, 0], [100, 100, 255], [150, 80, 80], [255, 200, 100], [0, 180, 255]]
FONT_COLORS = [[140, 240, 0], [80, 80, 255], [190, 50, 50], [255, 180, 80], [0, 180, 255]]
# FONT_COLORS = [[0, 255, 0], [0, 0, 255], [200, 60, 60], [255, 255, 0], [0, 255, 255]]

# cv2 input, output, para ...
capture = cv2.VideoCapture("../videos/ori.avi")
w = int(capture.get(3))
h = int(capture.get(4))
fourcc = cv2.VideoWriter_fourcc(*'mp4v')
out = cv2.VideoWriter("../test27.mp4", fourcc, 30, (w, h))

# in order to draw ball track
pts = [deque(maxlen=30) for _ in range(99)]
time_since_update = {}
for _ in range(60):
    time_since_update[str(_)] = 100


# smooth id 
# smooth id 
posX = np.zeros((5, 1097))
posY = np.zeros((5, 1097))
radius = np.zeros((5, 1097))

smooT =[0, 0, 0, 0, 0]
# 0 - 1096
# no any process, only read dict to np
for kFrame, vFrame in personDict.items():
    # 1 - 5
    for kPerson, vPerson in vFrame.items():
        dx = (vPerson[2] - vPerson[0]) / 2
        dy = (vPerson[3] - vPerson[1]) / 2
        dist = np.sqrt(dx ** 2 +  dy ** 2)
        x = (vPerson[0] + vPerson[2]) / 2
        y = (vPerson[1] + vPerson[3]) / 2
        posX[int(kPerson) - 1][int(kFrame)] = x
        posY[int(kPerson) - 1][int(kFrame)] = y
        radius[int(kPerson) - 1][int(kFrame)] = dist
        


plt.figure(figsize=(20,10)) #设置窗口大小
plt.suptitle('Muti-Image')
rols = 6
cols = 3
countPlt = 1
personId = 0

smoothSize = 20
for i in [0, 1, 2, 3, 4]:
    for j in range(3, 1094, 1):
        sumX = 0
        sumY = 0
        sumR = 0
        numX = 0
        numY = 0
        numR = 0
        # 不管距离 只对未检测到的帧进行处理
        if posX[i][j] == 0:
            for _ in [-3, -2, -1, 0, 1, 2, 3]:
                if posX[i][j + _] != 0:
                    sumX = sumX + posX[i][j + _]
                    numX = numX + 1
            posX[i][j] = sumX / numX
        if posY[i][j] == 0:
            for _ in [-3, -2, -1, 0, 1, 2, 3]:
                if posY[i][j + _] != 0:
                    sumY = sumY + posY[i][j + _]
                    numY = numY + 1
            posY[i][j] = sumY / numY
        if radius[i][j] == 0:
            for _ in [-3, -2, -1, 0, 1, 2, 3]:
                if radius[i][j + _] != 0:
                    sumR = sumR + radius[i][j + _]
                    numR = numR + 1
            radius[i][j] = sumR / numR

plt.subplot(rols, cols, countPlt)
countPlt = countPlt + 1
plt.title('ori-x')
plt.plot(range(1097), posX[personId])

plt.subplot(rols, cols, countPlt)
countPlt = countPlt + 1
plt.title('ori-y')
plt.plot(range(1097), posY[personId])

plt.subplot(rols, cols, countPlt)
countPlt = countPlt + 1
plt.title('ori-xy')
plt.plot(posX[personId], posY[personId])


"""
# 三次函数拟合平滑 7点
outX = np.zeros((5, 1097))
outY = np.zeros((5, 1097))
for i in [0, 1, 2, 3, 4]:
    outX[i][0] = (39.0 * posX[i][0] + 8.0 * posX[i][1] - 4.0 * posX[i][2] - 4.0 * posX[i][3] + posX[i][4] * 1.0 + posX[i][5] * 4.0 - posX[i][6] * 2.0) / 42
    outX[i][1] = (8.0 * posX[i][0] + 19.0 * posX[i][1] + 16.0 * posX[i][2] + 6.0 * posX[i][3] - 4.0 * posX[i][4] - 7.0* posX[i][5] + 4.0 * posX[i][6]) / 42.0
    outX[i][2] = (-4.0 * posX[i][0] + 16.0 * posX[i][1] + 19.0 * posX[i][2] + 12.0 * posX[i][3] + 2.0 * posX[i][4] - 4.0 * posX[i][5] + 1.0 * posX[i][6]) / 42.0
    outY[i][0] = (39.0 * posY[i][0] + 8.0 * posY[i][1] - 4.0 * posY[i][2] - 4.0 * posY[i][3] + posY[i][4] * 1.0 + posY[i][5] * 4.0 - posY[i][6] * 2.0) / 42
    outY[i][1] = (8.0 * posY[i][0] + 19.0 * posY[i][1] + 16.0 * posY[i][2] + 6.0 * posY[i][3] - 4.0 * posY[i][4] - 7.0* posY[i][5] + 4.0 * posY[i][6]) / 42.0
    outY[i][2] = (-4.0 * posY[i][0] + 16.0 * posY[i][1] + 19.0 * posY[i][2] + 12.0 * posY[i][3] + 2.0 * posY[i][4] - 4.0 * posY[i][5] + 1.0 * posY[i][6]) / 42.0

    for j in range(3, 1094, 1):
        outX[i][j] = (-2.0 * (posX[i][j - 3] + posX[i][j + 3]) + 3.0 * (posX[i][j - 2] + posX[i][j + 2]) + 6.0 * (posX[i][j - 1] + posX[i][j + 1]) + 7.0 * posX[i][j] ) / 21.0 
        outY[i][j] = (-2.0 * (posY[i][j - 3] + posY[i][j + 3]) + 3.0 * (posY[i][j - 2] + posY[i][j + 2]) + 6.0 * (posY[i][j - 1] + posY[i][j + 1]) + 7.0 * posY[i][j] ) / 21.0 

    outX[i][1096] = (39.0 * posX[i][0] + 8.0 * posX[i][1] - 4.0 * posX[i][2] - 4.0 * posX[i][3] + posX[i][4] * 1.0 + posX[i][5] * 4.0 - posX[i][6] * 2.0) / 42
    outX[i][1095] = (8.0 * posX[i][0] + 19.0 * posX[i][1] + 16.0 * posX[i][2] + 6.0 * posX[i][3] - 4.0 * posX[i][4] - 7.0* posX[i][5] + 4.0 * posX[i][6]) / 42.0
    outX[i][1094] = (-4.0 * posX[i][0] + 16.0 * posX[i][1] + 19.0 * posX[i][2] + 12.0 * posX[i][3] + 2.0 * posX[i][4] - 4.0 * posX[i][5] + 1.0 * posX[i][6]) / 42.0
    outY[i][1096] = (39.0 * posY[i][0] + 8.0 * posY[i][1] - 4.0 * posY[i][2] - 4.0 * posY[i][3] + posY[i][4] * 1.0 + posY[i][5] * 4.0 - posY[i][6] * 2.0) / 42
    outY[i][1095] = (8.0 * posY[i][0] + 19.0 * posY[i][1] + 16.0 * posY[i][2] + 6.0 * posY[i][3] - 4.0 * posY[i][4] - 7.0* posY[i][5] + 4.0 * posY[i][6]) / 42.0
    outY[i][1094] = (-4.0 * posY[i][0] + 16.0 * posY[i][1] + 19.0 * posY[i][2] + 12.0 * posY[i][3] + 2.0 * posY[i][4] - 4.0 * posY[i][5] + 1.0 * posY[i][6]) / 42.0
"""
# 拉绳
distX = 20
distY = 3
distR = 3
for i in [0, 1, 2, 3, 4]:
    for j in range(0, 1096, 1):
        distX = radius[i][j] / 8
        distY = radius[i][j] / 32
        distR = radius[i][j] / 16

        dist_temp = posX[i][j + 1] - posX[i][j]
        if abs(dist_temp) > distX:
            if dist_temp < 0:
                posX[i][j + 1] = posX[i][j] - distX
            else:
                posX[i][j + 1] = posX[i][j] + distX
        
        dist_temp = posY[i][j + 1] - posY[i][j]
        if abs(dist_temp) > distY:
            if dist_temp < 0:
                posY[i][j + 1] = posY[i][j] - distY
            else:
                posY[i][j + 1] = posY[i][j] + distY

        dist_temp = radius[i][j + 1] - radius[i][j]
        if abs(dist_temp) > distR:
            if dist_temp < 0:
                radius[i][j + 1] = radius[i][j] - distR
            else:
                radius[i][j + 1] = radius[i][j] + distR


plt.subplot(rols, cols, countPlt)
countPlt = countPlt + 1
plt.title('X-limit')
plt.plot(range(1097), posX[personId])

plt.subplot(rols, cols, countPlt)
countPlt = countPlt + 1
plt.title('Y-limit')
plt.plot(range(1097), posY[personId])

plt.subplot(rols, cols, countPlt)
countPlt = countPlt + 1
plt.title('X-Y-limit')
plt.plot(posX[personId], posY[personId])



# 单方向
for i in [0, 1, 2, 3, 4]:
    for j in range(1, 1096, 1):
        if posX[i][j] < posX[i][j + 1] and posX[i][j] < posX[i][j - 1]:
            posX[i][j] = (posX[i][j - 1] + posX[i][j + 1]) / 2
        elif posX[i][j] > posX[i][j + 1] and posX[i][j] > posX[i][j - 1]:
            posX[i][j] = (posX[i][j - 1] + posX[i][j + 1]) / 2

        if posY[i][j] < posY[i][j + 1] and posY[i][j] < posY[i][j - 1]:
            posY[i][j] = (posY[i][j - 1] + posY[i][j + 1]) / 2
        elif posY[i][j] > posY[i][j + 1] and posY[i][j] > posY[i][j - 1]:
            posY[i][j] = (posY[i][j - 1] + posY[i][j + 1]) / 2

        if radius[i][j] < radius[i][j + 1] and radius[i][j] < radius[i][j - 1]:
            radius[i][j] = (radius[i][j - 1] + radius[i][j + 1]) / 2
        elif radius[i][j] > radius[i][j + 1] and radius[i][j] > radius[i][j - 1]:
            radius[i][j] = (radius[i][j - 1] + radius[i][j + 1]) / 2

plt.subplot(rols, cols, countPlt)
countPlt = countPlt + 1
plt.title('X-single')
plt.plot(range(1097), posX[personId])

plt.subplot(rols, cols, countPlt)
countPlt = countPlt + 1
plt.title('Y-single')
plt.plot(range(1097), posY[personId])

plt.subplot(rols, cols, countPlt)
countPlt = countPlt + 1
plt.title('X-Y-single')
plt.plot(posX[personId], posY[personId])


# 二次函数拟合平滑 7点
outX = np.zeros((5, 1097))
outY = np.zeros((5, 1097))
for i in [0, 1, 2, 3, 4]:
    outX[i][0] = (32.0 * posX[i][0] + 15.0 * posX[i][1] + 3.0 * posX[i][2] - 4.0 * posX[i][3] - 6.0 * posX[i][4] - 3.0 * posX[i][5] + 5.0 * posX[i][6]) / 42.0
    outX[i][1] = (5.0 * posX[i][0] + 4.0 * posX[i][1] + 3.0 * posX[i][2] + 2.0 * posX[i][3] + 1.0 * posX[i][4] - 1.0 * posX[i][5] + 1.0 * posX[i][6]) / 15.0
    outX[i][2] = (1.0 * posX[i][0] + 3.0 * posX[i][1] + 4.0 * posX[i][2] + 4.0 * posX[i][3] + 3.0 * posX[i][4] + 1.0 * posX[i][5] - 2.0 * posX[i][6]) / 14.0
    outY[i][0] = (32.0 * posY[i][0] + 15.0 * posY[i][1] + 3.0 * posY[i][2] - 4.0 * posY[i][3] - 6.0 * posY[i][4] - 3.0 * posY[i][5] + 5.0 * posY[i][6]) / 42.0
    outY[i][1] = (5.0 * posY[i][0] + 4.0 * posY[i][1] + 3.0 * posY[i][2] + 2.0 * posY[i][3] + 1.0 * posY[i][4] - 1.0 * posY[i][5] + 1.0 * posY[i][6]) / 15.0
    outY[i][2] = (1.0 * posY[i][0] + 3.0 * posY[i][1] + 4.0 * posY[i][2] + 4.0 * posY[i][3] + 3.0 * posY[i][4] + 1.0 * posY[i][5] - 2.0 * posY[i][6]) / 14.0

    for j in range(3, 1094, 1):
        outX[i][j] = (-2.0 * (posX[i][j - 3] + posX[i][j + 3]) + 3.0 * (posX[i][j - 2] + posX[i][j + 2]) + 6.0 * (posX[i][j - 1] + posX[i][j + 1]) + 7.0 * posX[i][j] ) / 21.0 
        outY[i][j] = (-2.0 * (posY[i][j - 3] + posY[i][j + 3]) + 3.0 * (posY[i][j - 2] + posY[i][j + 2]) + 6.0 * (posY[i][j - 1] + posY[i][j + 1]) + 7.0 * posY[i][j] ) / 21.0 

    outX[i][1096] = (32.0 * posX[i][1096] + 15.0 * posX[i][1095] + 3.0 * posX[i][1094] - 4.0 * posX[i][1093] - 6.0 * posX[i][1092] - 3.0 * posX[i][1091] + 5.0 * posX[i][1090]) / 42.0
    outX[i][1095] = (5.0 * posX[i][1096] + 4.0 * posX[i][1095] + 3.0 * posX[i][1094] + 2.0 * posX[i][1093] + 1.0 * posX[i][1092] - 1.0 * posX[i][1091] + 1.0 * posX[i][1090]) / 15.0
    outX[i][1094] = (1.0 * posX[i][1096] + 3.0 * posX[i][1095] + 4.0 * posX[i][1094] + 4.0 * posX[i][1093] + 3.0 * posX[i][1092] + 1.0 * posX[i][1091] - 2.0 * posX[i][1090]) / 14.0
    outY[i][1096] = (32.0 * posY[i][1096] + 15.0 * posY[i][1095] + 3.0 * posY[i][1094] - 4.0 * posY[i][1093] - 6.0 * posY[i][1092] - 3.0 * posY[i][1091] + 5.0 * posY[i][1090]) / 42.0
    outY[i][1095] = (5.0 * posY[i][1096] + 4.0 * posY[i][1095] + 3.0 * posY[i][1094] + 2.0 * posY[i][1093] + 1.0 * posY[i][1092] - 1.0 * posY[i][1091] + 1.0 * posY[i][1090]) / 15.0
    outY[i][1094] = (1.0 * posY[i][1096] + 3.0 * posY[i][1095] + 4.0 * posY[i][1094] + 4.0 * posY[i][1093] + 3.0 * posY[i][1092] + 1.0 * posY[i][1091] - 2.0 * posY[i][1090]) / 14.0


plt.subplot(rols, cols, countPlt)
countPlt = countPlt + 1
plt.title('fuc-smooth-x')
plt.plot(range(1097), outX[personId])

plt.subplot(rols, cols, countPlt)
countPlt = countPlt + 1
plt.title('fuc-smooth-y')
plt.plot(range(1097), outY[personId])

plt.subplot(rols, cols, countPlt)
countPlt = countPlt + 1
plt.title('fuc-smooth-xy')
plt.plot(outX[personId], outY[personId])



# 线性平滑 7点
N = 1097
outX = np.zeros((5, 1097))
outY = np.zeros((5, 1097))
for i in [0, 1, 2, 3, 4]:
    outX[i][0] = (5 * posX[i][0] + 4 * posX[i][1] + 3 * posX[i][2] + 2 * posX[i][3] + posX[i][4] + posX[i][5] + posX[i][6]) / 17.0
    outX[i][1] = (4 * posX[i][0] + 5 * posX[i][1] + 4 * posX[i][2] + 3 * posX[i][3] + 2 * posX[i][4] + posX[i][5] + posX[i][6]) / 20.0
    outX[i][2] = (3 * posX[i][0] + 4 * posX[i][1] + 5 * posX[i][2] + 4 * posX[i][3] + 3 * posX[i][4] + 2 * posX[i][5] + 1 * posX[i][6]) / 22.0

    outY[i][0] = (5 * posY[i][0] + 4 * posY[i][1] + 3 * posY[i][2] + 2 * posY[i][3] + posY[i][4] + posY[i][5] + posY[i][6]) / 17.0
    outY[i][1] = (4 * posY[i][0] + 5 * posY[i][1] + 4 * posY[i][2] + 3 * posY[i][3] + 2 * posY[i][4] + posY[i][5] + posY[i][6]) / 20.0
    outY[i][2] = (3 * posY[i][0] + 4 * posY[i][1] + 5 * posY[i][2] + 4 * posY[i][3] + 3 * posY[i][4] + 2 * posY[i][5] + 1 * posY[i][6]) / 22.0

    for j in range(3, 1094, 1):
        outX[i][j] = (posX[i][j - 3] + posX[i][j + 3] + posX[i][j - 2] + posX[i][j + 2] + posX[i][j - 1] + posX[i][j + 1] + posX[i][j] ) / 7
        outY[i][j] = (posY[i][j - 3] + posY[i][j + 3] + posY[i][j - 2] + posY[i][j + 2] + posY[i][j - 1] + posY[i][j + 1] + posY[i][j] ) / 7

    outX[i][N - 1] = (5 * posX[i][N - 1] + 4 * posX[i][N - 2] + 3 * posX[i][N - 3] + 2 * posX[i][N - 4] + posX[i][N - 5] + posX[i][N - 6] + posX[i][N - 7]) / 17.0
    outX[i][N - 2] = (4 * posX[i][N - 1] + 5 * posX[i][N - 2] + 4 * posX[i][N - 3] + 3 * posX[i][N - 4] + 2 * posX[i][N - 5] + posX[i][N - 6] + posX[i][N - 7]) / 20.0
    outX[i][N - 3] = (3 * posX[i][N - 1] + 4 * posX[i][N - 2] + 5 * posX[i][N - 3] + 4 * posX[i][N - 4] + 3 * posX[i][N - 5] + 2 * posX[i][N - 6] + 1 * posX[i][N - 7]) / 22.0

    outY[i][N - 1] = (5 * posY[i][N - 1] + 4 * posY[i][N - 2] + 3 * posY[i][N - 3] + 2 * posY[i][N - 4] + posY[i][N - 5] + posY[i][N - 6] + posY[i][N - 7]) / 17.0
    outY[i][N - 2] = (4 * posY[i][N - 1] + 5 * posY[i][N - 2] + 4 * posY[i][N - 3] + 3 * posY[i][N - 4] + 2 * posY[i][N - 5] + posY[i][N - 6] + posY[i][N - 7]) / 20.0
    outY[i][N - 3] = (3 * posY[i][N - 1] + 4 * posY[i][N - 2] + 5 * posY[i][N - 3] + 4 * posY[i][N - 4] + 3 * posY[i][N - 5] + 2 * posY[i][N - 6] + 1 * posY[i][N - 7]) / 22.0

plt.subplot(rols, cols, countPlt)
countPlt = countPlt + 1
plt.title('line-smooth-x')
plt.plot(range(1097), outX[personId])

plt.subplot(rols, cols, countPlt)
countPlt = countPlt + 1
plt.title('line-smooth-y')
plt.plot(range(1097), outY[personId])

plt.subplot(rols, cols, countPlt)
countPlt = countPlt + 1
plt.title('line-smooth-xy')
plt.plot(outX[personId], outY[personId])


N = 1097
# 平滑 100 帧
queX = [deque(maxlen=100) for _ in range(10)]
queY = [deque(maxlen=200) for _ in range(10)]

"""
for i in [0, 1, 2, 3, 4]:
    for j in range(N):
        lenQueX = len(queX[i])
        if lenQueX == 0:
            queX.append(outX[i][j])
            continue
        ansX = 0
        for k in queX[i]:
            ansX = ansX + k
        ansX = ansX + outX[i][j]
        outX[i][j] = ansX / (lenQueX + 1)
        queX.append(outX[i][j])

        lenQueY = len(queY[i])
        if lenQueY == 0:
            queY.append(outY[i][j])
            continue
        ansY = 0
        for k in queY[i]:
            ansY = ansY + k
        ansY = ansY + outY[i][j]
        outY[i][j] = ansY / (lenQueY + 1)
        queY.append(outY[i][j])
"""
for i in [0, 1, 2, 3, 4]:
    for j in range(N):
        lenQueX = len(queX[i])
        if lenQueX == 0:
            queX.append(outX[i][j])
            continue
        ansX = 0
        for k in queX[i]:
            ansX = ansX + k
        ansX = ansX + outX[i][j]
        queX.append(outX[i][j])
        outX[i][j] = ansX / (lenQueX + 1)

        lenQueY = len(queY[i])
        if lenQueY == 0:
            queY.append(outY[i][j])
            continue
        ansY = 0
        for k in queY[i]:
            ansY = ansY + k
        ansY = ansY + outY[i][j]
        queY.append(outY[i][j])
        outY[i][j] = ansY / (lenQueY + 1)


plt.subplot(rols, cols, countPlt)
countPlt = countPlt + 1
plt.title('100-smooth-x')
plt.plot(range(1097), outX[personId])

plt.subplot(rols, cols, countPlt)
countPlt = countPlt + 1
plt.title('100-smooth-y')
plt.plot(range(1097), outY[personId])

plt.subplot(rols, cols, countPlt)
countPlt = countPlt + 1
plt.title('100-smooth-xy')
plt.plot(outX[personId], outY[personId])





plt.show()
# 逐帧遍历 videos, write some json in it
# 0 - 1096 
frame_num = 0
smooT2 = [0, 0 ,0, 0, 0]
while True:
    #print("frame: {%d}"%frame_num)
    ret, frame = capture.read()
    if ret != True:
        break
    oriFrame = frame.copy()
    
    cv2.putText(oriFrame, "Frame ", (int(5), int(5)), 0, 5e-3 * 200, (200, 200, 200), 4)
    
    # read json write in frame
    ballInfo = ballDict[str(frame_num)]
    personInfo = personDict[str(frame_num)]

    # 画球
    for kBall, vBall in ballInfo.items():
        if len(vBall) == 0:
            continue
        track_id = int(kBall)
        idBall = int(int(kBall) / 10)
        color = COLORS[idBall - 1]
        # 小球的框
        # cv2.rectangle(frame, (int(vBall[0]), int(vBall[1])), (vBall[2], vBall[3]), color, 3)
        # 小球的轨迹
        center = (int((vBall[0] + vBall[2]) / 2), int((vBall[1] + vBall[3]) / 2))
        time_since_update[str(track_id)] = 0
        pts[track_id].append(center)

    for i in range(60):
        time_since_update[str(i)] += 1
        if(time_since_update[str(i)] >= 3):
            for _ in range(3):
                if len(pts[i]) == 0:
                    break
                pts[i].popleft()
    
    for _id in range(60):
        colorB = COLORS[int(_id / 10) - 1].copy()
        if len(pts[_id]) == 0:
            continue
        for j in range(len(pts[_id]) - 1, -1, -1):
            thickness = int(np.sqrt(20 + j * 1))
            for _c in range(3):
                if colorB[_c] < 200:
                    colorB[_c] = colorB[_c] + 2
                else:
                    colorB[_c] = colorB[_c] - 2
            cv2.circle(frame, (pts[_id][j]), thickness, (colorB),  -1)


    # 画人的ID()
    for i in [0, 1, 2, 3, 4]:
        # 透明操作
       # colorG = COLORS[int(kPerson) - 1]
       # if frame_num > 50 and frame_num < 75 and kPerson == '3':
       #     cv2.circle(oriFrame, (xx, yy), int(dist), colorG, 10)  # 圆框
       #     cv2.circle(oriFrame, (xx, yy), int(dist), (255, 255, 255), -1)  # 圆内部
       # elif frame_num > 280 and frame_num < 350 and kPerson == '5':
       #     cv2.circle(oriFrame, (xx, yy), int(dist), colorG, 10)  # 圆框
       #     cv2.circle(oriFrame, (xx, yy), int(dist), (255, 255, 255), -1)  # 圆内部
       # elif frame_num > 1045 and kPerson == '2':
       #     cv2.circle(oriFrame, (xx, yy), int(dist), colorG, 10)  # 圆框
       #     cv2.circle(oriFrame, (xx, yy), int(dist), (255, 255, 255), -1)  # 圆内部
         
        #平滑处理
        xText = outX[i][frame_num]
        yText = outY[i][frame_num] - 5
        colorT = FONT_COLORS[i]
        # cv2.rectangle(frame, (int(vPerson[0]), int(vPerson[1])), (int(vPerson[2]), int(vPerson[3])), colorT, 3)
        cv2.putText(frame, str(i + 1), (int(xText), int(yText)), 0, 5e-3 * 250, (colorT), 4)
        cv2.putText(oriFrame, str(i + 1), (int(xText), int(yText)), 0, 5e-3 * 250, (colorT), 4)

    img_mix = cv2.addWeighted(frame, 0.8, oriFrame, 0.4, 0)
    cv2.imshow("test", img_mix)
    out.write(img_mix) 
    frame_num = frame_num + 1
    if cv2.waitKey(1) & 0xff == ord('q'):
        break
out.release()
